#ifndef NAVIGATION_H
#define NAVIGATION_H

#include "uavmath.h"

typedef struct nav_state_t {
	vector3_t p; // position
	vector3_t v; // velocity
	quaternion_t q; // orientation
	vector3_t bw; // gyro biases
	vector3_t ba; // accelerometer biases
} nav_state_t;

// Updates the navigation state x given the IMU data and time step.
void nav_update(nav_state_t *x, const vector3_t *w, const vector3_t *a, fixed_t dt);

extern vector3_t NAV_IMU_OFFSET; // IMU offset from center-of-gravity
extern fixed_t NAV_G; // gravitational acceleration constant

#endif
